package org.eoti.ai.pathfinding.astar;

/*
 * The MIT License
 *
 * Copyright (c) 2009 Malachi de AElfweald <malachid@gmail.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

import java.util.Comparator;
import java.util.PriorityQueue;
import java.util.concurrent.CopyOnWriteArrayList;
import java.util.concurrent.locks.ReentrantReadWriteLock;

public class AStar<NODE extends AStarNode<NODE, LOCATION>, HEURISTIC extends AStarHeuristic<NODE,LOCATION>, MOVEMENT extends AStarMovement<NODE,LOCATION>, LOCATION>
{
	protected ReentrantReadWriteLock lock = new ReentrantReadWriteLock();
	protected Comparator<NODE> comparator;
	protected HEURISTIC heuristic;
	protected MOVEMENT movement;
	protected PriorityQueue<NODE> open;
	protected CopyOnWriteArrayList<NODE> closed;
	protected NODE startNode, endNode;

	public AStar(
			Comparator<NODE> comparator,
			HEURISTIC heuristic,
			MOVEMENT movement
	)
	{
		lock.writeLock().lock();
		this.comparator = comparator;
		this.heuristic = heuristic;
		this.movement = movement;
		this.open = new PriorityQueue<NODE>(1, comparator);//higher than 1 would be more efficient
		this.closed = new CopyOnWriteArrayList<NODE>();
		lock.writeLock().unlock();
	}

	public HEURISTIC getHeuristic(){return heuristic;}
	public MOVEMENT getMovement(){return movement;}
	public NODE getStartNode(){return startNode;}
	public NODE getEndNode(){return endNode;}

	public void setGoal(NODE startNode, NODE endNode)
	{
		lock.writeLock().lock();
		this.startNode = startNode;
		this.endNode = endNode;

		open.clear();
		closed.clear();

		open.add(startNode);
		lock.writeLock().unlock();
	}

	public NODE findPath()
	{
		lock.writeLock().lock();
		while(!open.isEmpty())
		{
			NODE current = open.poll();
			if(current.getLocation().equals(endNode.getLocation()))
			{
				// done
				return current;
			}

			for(NODE neighbor : current.getNeighbors())
			{
				double throughUs = current.getActualCost() + movement.movementCost(current, neighbor);
				if(open.contains(neighbor))
				{
					if(throughUs < neighbor.getActualCost())
					{
						neighbor.setParent(current);
						neighbor.setActualCost(throughUs);
					}
				}else if(closed.contains(neighbor)){
					if(throughUs < neighbor.getActualCost())
					{
						closed.remove(neighbor);
						neighbor.setParent(current);
						open.add(neighbor);
					}
				}else{
					neighbor.setParent(current);
					neighbor.setActualCost(throughUs);
					open.add(neighbor);
				}
			}
		}

		lock.writeLock().unlock();
		return null;
	}
}
